module nade.collision;

import std.math;

import derelict.opengl.gl;

import nade.vector;
import nade.angle;
import nade.location;
import nade.color;
import nade.matrix;
import nade.primitive;
import nade.space;
import nade.physics;


class CollisionHull {
	this(Vector2[] vertices ...)
	{
		_vertices = vertices.dup;
	}
	this(Vector2[] vertices, Vector3[] clippers)
	{
		_vertices = vertices;
	}

	void render()
	{
		primitive(GL_LINE_LOOP, _vertices);
	}

	private {
		Vector2[]	_vertices;
	}
}


class CollisionObject : Locatable {
	alias void delegate(const Vector2 position, const Vector2 penetration, const Vector2 impulse) CollisionDg;

	this(CollisionHull hull, real bounce, const Matrix2x3 offset = Matrix2x3.IDENTITY)
	{
		_hull = hull;
		_bounce = bounce;
		_offset = offset;
		_vertices.length = _clippers.length = hull._vertices.length;
	}

	override void attached()
	{
		moved;
	}

	override void moved()
	{
		assert(location !is null);
		const transform = location.toMatrix2x3*offset;
		Vector2 prev = transform*_hull._vertices[$ - 1];
		foreach(k, vertex; _hull._vertices)
		{
			const t = transform*vertex;
			_vertices[k] = t;
			const rn = (t - prev).right.normal;
			_clippers[k] = Vector3(rn.x, rn.y, rn.dot(t));
			prev = t;
		}
	}

	void render()
	{
		primitive(GL_LINE_LOOP, _vertices);

		// glBegin(GL_LINES);
		// foreach(clipper; _clippers)
		// {
			// glVertex2f(clipper.x*clipper.z - clipper.y, clipper.y*clipper.z + clipper.x);
			// glVertex2f(clipper.x*clipper.z + clipper.y, clipper.y*clipper.z - clipper.x);
			// glVertex2f(clipper.x*clipper.z, clipper.y*clipper.z);
			// glVertex2f(clipper.x*clipper.z + clipper.x*0.1, clipper.y*clipper.z + clipper.y*0.1);
		// }
		// glEnd();
	}

	void collide(CollisionObject other)
	{
		if(_physicsObject is null  &&  other._physicsObject is null) return;

		real depth = real.max;
		Vector2 position;
		Vector2 normal;
		foreach(clipper; _clippers)
		{
			uint dk;
			real min = real.max;
			foreach(k, vertex; other._vertices)
			{
				const d = clipper.dot(vertex) - clipper.z;
				if(d < min)
				{
					min = d;
					dk = k;
				}
			}

			if(min >= 0) return;
			if(min > -depth)
			{
				depth = -min;
				position = other._vertices[dk];
				normal = clipper.xy;
			}
		}
		foreach(clipper; other._clippers)
		{
			uint dk;
			real min = real.max;
			foreach(k, vertex; _vertices)
			{
				const d = clipper.dot(vertex) - clipper.z;
				if(d < min)
				{
					min = d;
					dk = k;
				}
			}

			if(min >= 0) return;
			if(min > -depth)
			{
				depth = -min;
				position = _vertices[dk];
				normal = -clipper.xy;
			}
		}

		const pm1 = (_physicsObject is null) ? Vector2.ZERO : _physicsObject.momentumAt(position);
		const pm2 = (other._physicsObject is null) ? Vector2.ZERO : other._physicsObject.momentumAt(position);
		const dm = normal.dot(pm1) - normal.dot(pm2);
		const impulse = (dm > 0) ? normal*dm : Vector2.ZERO;

		const bounce = (dm > 0.0001) ? (1 + fmax(_bounce, other._bounce)) : 0;

		const real m1 = (_physicsObject is null) ? 0 : 1;
		const real m2 = (other._physicsObject is null) ? 0 : 1;
		const f1 = -m1/(m1 + m2);
		const f2 = m2/(m1 + m2);

		const penetration = normal*depth;
		if(_physicsObject !is null) _physicsObject.collision(position, f1*penetration, f1*bounce*impulse);
		if(other._physicsObject !is null) other._physicsObject.collision(position, f2*penetration, f2*bounce*impulse);

		glPointSize(8);
		primitive(GL_POINTS, position);
		glPointSize(1);

		glLineWidth(4);
		primitive(GL_LINES, position, position + normal*depth*5);
		glLineWidth(1);
	}

	PhysicsObject physicsObject() { return _physicsObject; }
	PhysicsObject physicsObject(PhysicsObject rhs) { return _physicsObject = rhs; }

	Matrix2x3 offset() { return _offset; }
	Matrix2x3 offset(const Matrix2x3 rhs) { _offset = rhs; moved(); return rhs; }

	private {
		CollisionHull		_hull;
		real				_bounce;
		PhysicsObject		_physicsObject;
		Matrix2x3			_offset;
		Vector2[]			_vertices;
		Vector3[]			_clippers;
	}
}


class CollisionSpace : Space!(CollisionObject) {
	void collide()
	{
		foreach(k, collider; _objects[0 .. ($ - 1)])
			foreach(other; _objects[(k + 1) .. $])
				collider.collide(other);
	}

	void render()
	{
		foreach(k, collider; _objects) collider.render();
	}

	CollisionObject spawn(CollisionHull hull, real bounce, const Matrix2x3 offset = Matrix2x3.IDENTITY)
	{
		return super.attach(new CollisionObject(hull, bounce, offset));
	}
}
